/** 
* @file am_demod.c
* @brief Functions to perform AM demodulation 
* @author Frank Brickle, AB2KT and Bob McGwier, N4HY 


This file is part of a program that implements a Software-Defined Radio.

Copyright (C) 2004, 2005, 2006, 2007, 2008 by Frank Brickle, AB2KT and Bob McGwier, N4HY
Doxygen comments added by Dave Larsen, KV0S

This program is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation; either version 3 of the License, or
(at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

The authors can be reached by email at

ab2kt@arrl.net
or
rwmcgwier@gmail.com

or by paper mail at

The DTTS Microwave Society
6 Kathleen Place
Bridgewater, NJ 08807
*/

#include <am_demod.h>

/*------------------------------------------------------------------------------*/
/* private to AM */
/*------------------------------------------------------------------------------*/

/* -------------------------------------------------------------------------- */
/** 
 * @brief Initialize a phased lock loop
* 
* @param am 
* @param samprate 
* @param freq 
* @param lofreq 
* @param hifreq 
* @param bandwidth 
*/
/* ---------------------------------------------------------------------------- */
PRIVATE void
init_pll(AMD am,
	 REAL samprate,
	 REAL freq,
	 REAL lofreq,
	 REAL hifreq,
	 REAL bandwidth) {
  REAL fac = (REAL) (TWOPI / samprate);
  am->pll.freq.f = freq * fac;
  am->pll.freq.l = lofreq * fac;
  am->pll.freq.h = hifreq * fac;
  am->pll.phs = 0.0;
  am->pll.delay = cxJ;

  am->pll.iir.alpha = bandwidth * fac;	/* arm filter */
  am->pll.alpha = am->pll.iir.alpha * 0.3f;	/* pll bandwidth */
  am->pll.beta = am->pll.alpha * am->pll.alpha * 0.25f;	/* second order term */
  am->pll.fast_alpha = am->pll.alpha;
}

/* -------------------------------------------------------------------------- */
/** @brief run phased lock loop 
* 
* @param am 
* @param sig 
*/
/* ---------------------------------------------------------------------------- */
PRIVATE void
pll(AMD am, COMPLEX sig) {
  COMPLEX z = Cmplx((REAL) cos(am->pll.phs), (REAL) sin(am->pll.phs));
  REAL diff;

  am->pll.delay.re = z.re * sig.re + z.im * sig.im;
  am->pll.delay.im = -z.im * sig.re + z.re * sig.im;
  diff = Cmag(sig) * ATAN2(am->pll.delay.im, am->pll.delay.re);

  am->pll.freq.f += am->pll.beta * diff;

  if (am->pll.freq.f < am->pll.freq.l)
    am->pll.freq.f = am->pll.freq.l;
  if (am->pll.freq.f > am->pll.freq.h)
    am->pll.freq.f = am->pll.freq.h;

  am->pll.phs += am->pll.freq.f + am->pll.alpha * diff;

  while (am->pll.phs >= TWOPI)
    am->pll.phs -= (REAL) TWOPI;
  while (am->pll.phs < 0)
    am->pll.phs += (REAL) TWOPI;
}

/* -------------------------------------------------------------------------- */
/** @brief Demodulate signal
* 
* @param am 
*/
/* ---------------------------------------------------------------------------- */
PRIVATE REAL
dem(AMD am) {
  am->lock.curr = 0.999 * am->lock.curr + 0.001 * fabs(am->pll.delay.im);
  am->lock.prev = am->lock.curr;
  am->dc = 0.9999f * am->dc + 0.0001f * am->pll.delay.re;
  return am->pll.delay.re - am->dc;
}

/*-----------------------------------------------------------------------------*/
/* public */ 
/*------------------------------------------------------------------------------*/

void
/* -------------------------------------------------------------------------- */
/** @brief AM Demodulation
* 
* @param am 
*/
/* ---------------------------------------------------------------------------- */
AMDemod(AMD am) {
  int i;
  REAL demout;
  switch (am->mode) {
  case SAMdet:
    for (i = 0; i < am->size; i++) {
      pll(am, CXBdata(am->ibuf, i));
      demout = dem(am);
      CXBdata(am->obuf, i) = Cmplx(demout, demout);
    }
    break;
  case AMdet:
    for (i = 0; i < am->size; i++) {
      am->lock.curr = Cmag(CXBdata(am->ibuf, i));
      am->dc = 0.9999f * am->dc + 0.0001f * am->lock.curr;
      am->smooth = 0.5f * am->smooth + 0.5f * (am->lock.curr - am->dc);
      /* demout = am->smooth; */
      CXBdata(am->obuf, i) = Cmplx(am->smooth, am->smooth);
    }
    break;
  }
}

AMD
/* -------------------------------------------------------------------------- */
/** @brief New AM demodulation object
* 
* @param samprate 
* @param f_initial 
* @param f_lobound 
* @param f_hibound 
* @param f_bandwid 
* @param size 
* @param ivec 
* @param ovec 
* @param mode 
* @param tag 
*/
/* ---------------------------------------------------------------------------- */
newAMD(REAL samprate,
       REAL f_initial,
       REAL f_lobound,
       REAL f_hibound,
       REAL f_bandwid,
       int size,
       COMPLEX *ivec,
       COMPLEX *ovec,
       AMMode mode,
       char *tag) {
  AMD am = (AMD) safealloc(1, sizeof(AMDDesc), tag);

  am->size = size;
  am->ibuf = newCXB(size, ivec, tag);
  am->obuf = newCXB(size, ovec, tag);
  am->mode = mode;
  init_pll(am, samprate, f_initial, f_lobound, f_hibound, f_bandwid);

  am->lock.curr = 0.5;
  am->lock.prev = 1.0;
  am->dc = 0.0;

  return am;
}

void
/* -------------------------------------------------------------------------- */
/** @brief delete an AM Demodulation object
* 
* @param am 
*/
/* ---------------------------------------------------------------------------- */
delAMD(AMD am) {
  if (am) {
    delCXB(am->ibuf);
    delCXB(am->obuf);
    safefree((char *) am);
  }
}
